// -*- Mode: C; c-basic-offset: 8; -*-
//
// Copyright (c) 2012 Andrew Tridgell, All Rights Reserved
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
//
//  o Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//  o Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in
//    the documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
// COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
// INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
// STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
// OF THE POSSIBILITY OF SUCH DAMAGE.
//

///
/// @file	mavlink.c
///
/// mavlink reporting code
///

#include <stdarg.h>
#include "radio.h"
#include "packet.h"
#include "timer.h"

extern  uint8_t pbuf[MAX_PACKET_LENGTH];
extern uint8_t seen_mavlink;
static  uint8_t seqnum;

// new RADIO_STATUS common message
#define MAVLINK_MSG_ID_RADIO_STATUS 109
#define MAVLINK_RADIO_STATUS_CRC_EXTRA 185

// PROTOCOL_VERSION message
#define MAVLINK_MSG_ID_PROTOCOL_VERSION 300
#define MAVLINK_PROTOCOL_VERSION_CRC_EXTRA 217

// use '3D' for 3DRadio
#define RADIO_SOURCE_SYSTEM '3'
#define RADIO_SOURCE_COMPONENT 'D'

/*
 * Calculates the MAVLink checksum on a packet in pbuf[] 
 * and append it after the data
 */
static void mavlink_crc(register uint8_t crc_extra)
{
	register uint8_t length = pbuf[1];
	 uint16_t sum = 0xFFFF;
	 uint8_t i, stoplen;

	uint8_t offset = (seen_mavlink == 1) ? 6 : 10;

	stoplen = length + offset;

	// MAVLink 1.0/2.0 has an extra CRC seed
	pbuf[length+offset] = crc_extra;
	stoplen++;

	i = 1;
	while (i<stoplen) {
		register uint8_t tmp;
		tmp = pbuf[i] ^ (uint8_t)(sum&0xff);
		tmp ^= (tmp<<4);
		sum = (sum>>8) ^ (tmp<<8) ^ (tmp<<3) ^ (tmp>>4);
		i++;
	}

	pbuf[length+offset] = sum&0xFF;
	pbuf[length+offset+1] = sum>>8;
}


/*
We use a hand-crafted MAVLink packet based on the following
message definition:

http://mavlink.org/messages/common#RADIO_STATUS

<message name="RADIO_STATUS" id="109">
	<description>Status generated by radio</description>
	<field type="uint8_t" name="rssi">local signal strength</field>
	<field type="uint8_t" name="remrssi">remote signal strength</field>
	<field type="uint8_t" name="txbuf">percentage free space in transmit buffer</field>
	<field type="uint8_t" name="noise">background noise level</field>
	<field type="uint8_t" name="remnoise">remote background noise level</field>
	<field type="uint16_t" name="rxerrors">receive errors</field>
	<field type="uint16_t" name="fixed">count of error corrected packets</field>
</message>

Note that the wire field ordering follows the MAVLink v1.0/v2.0 spec

*/
struct mavlink_RADIO_v10 {
	uint16_t rxerrors;
	uint16_t fixed;
	uint8_t rssi;
	uint8_t remrssi;
	uint8_t txbuf;
	uint8_t noise;
	uint8_t remnoise;
};

struct mavlink_PROTOCOL_VERSION_v10 {
	uint16_t version;
	uint16_t min_version;
	uint16_t max_version;
	uint8_t spec_version_hash[8];
	uint8_t library_version_hash[8];
};

/// send a MAVLink status report packet
/// we send the same MAVLink version as received for RADIO_STATUS
/// and respond correctly to the protocol handshake sequence
/// https://mavlink.io/en/mavlink-version.html#version-handshaking
bool MAVLink_report(void)
{
	struct mavlink_RADIO_v10 *m;
	uint8_t full_packet_size;

	if (seen_mavlink == 1) {
		full_packet_size = sizeof(struct mavlink_RADIO_v10)+8;
	} else if (seen_mavlink == 2) {
		full_packet_size = sizeof(struct mavlink_RADIO_v10)+12;
	} else {
		full_packet_size = sizeof(struct mavlink_PROTOCOL_VERSION_v10)+12;
	}

	if (serial_write_space() < full_packet_size) {
		// don't cause an overflow
		return false;
	}

	// MAVLink 1.0 encoding
	if (seen_mavlink == 1) {
		m = (struct mavlink_RADIO_v10 *)&pbuf[6];
		pbuf[0] = MAVLINK10_STX;
		pbuf[1] = sizeof(struct mavlink_RADIO_v10);
		pbuf[2] = seqnum++;
		pbuf[3] = RADIO_SOURCE_SYSTEM;
		pbuf[4] = RADIO_SOURCE_COMPONENT;
		pbuf[5] = MAVLINK_MSG_ID_RADIO_STATUS;

	// MAVLink 2.0 encoding
	} else {
		m = (struct mavlink_RADIO_v10 *)&pbuf[10];
		pbuf[0] = MAVLINK20_STX;
		pbuf[1] = sizeof(struct mavlink_RADIO_v10);
		pbuf[2] = 0;
		pbuf[3] = 0;
		pbuf[4] = seqnum++;
		pbuf[5] = RADIO_SOURCE_SYSTEM;
		pbuf[6] = RADIO_SOURCE_COMPONENT;

		// If this is a protocol version request,
		// respond accordingly
		if (seen_mavlink == 3) {
			pbuf[7] = MAVLINK_MSG_ID_PROTOCOL_VERSION & 0xFF;
			pbuf[8] = (MAVLINK_MSG_ID_PROTOCOL_VERSION >> 8) & 0xFF;
		} else {
			pbuf[7] = MAVLINK_MSG_ID_RADIO_STATUS & 0xFF;
			pbuf[8] = 0;
		}
		// Never reaching the last 4 bit with any ID
		pbuf[9] = 0;
	}

	// PROTOCOL_VERSION
	if (seen_mavlink == 3) {
		struct mavlink_PROTOCOL_VERSION_v10 *v = (struct mavlink_PROTOCOL_VERSION_v10 *)&pbuf[10];
		v->min_version = 1;
		v->max_version = 2;
		memset(&v->spec_version_hash[0], 0, 8);
		memset(&v->library_version_hash[0], 0, 8);
		mavlink_crc(MAVLINK_PROTOCOL_VERSION_CRC_EXTRA);

	// RADIO_STATUS
	} else {
		m->rxerrors = errors.rx_errors;
		m->fixed    = errors.corrected_packets;
		m->txbuf    = serial_read_space();
		m->rssi     = statistics.average_rssi;
		m->remrssi  = remote_statistics.average_rssi;
		m->noise    = statistics.average_noise;
		m->remnoise = remote_statistics.average_noise;
		mavlink_crc(MAVLINK_RADIO_STATUS_CRC_EXTRA);
	}

	serial_write_buf(pbuf, full_packet_size);

	return true;
}
